#include "C_H.h"


#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <dirent.h>

#define Data_num 37

//int data1=1,data2=2,data3=3,data4=0,data5=0,data6=0,data7=0,data8=0,data9=0,data10=0,data11=0,data12=0,data13=0,data14=0,data15=0,data16=0,data17=0,data18=0,data19=0,data20=0,data21=0,data22=0;

int img_index=1;
uint8_t sd_first = 0;
using namespace cv;

// 存储从txt文件读取的数据（共30个值）
float dataValues[Data_num] = {0}; 

void Binary_image_save(int index)
{
    std::string folderPath = "./origin";
    // 生成文件路径
    std::string filePath = folderPath + "/" + std::to_string(index) + ".bmp";


    cv::Mat binImage(LCDH, LCDW, CV_8UC1);  // 创建一个 MT9V03X_HxMT9V03X_W 的单通道图像

    // 复制数据到 cv::Mat
    for (int i = 0; i < LCDH; ++i) {
        for (int j = 0; j < LCDW; ++j) {
          binImage.at<uchar>(69-i, 185-j) = BinPixels[i][j] * 255;  // 0 或 1 转换为 0 或 255
     }
    }
    cv::imwrite(filePath, binImage);
}

void Fitting_image_save(int index)
{
    // 创建一个 MT9V03X_HxMT9V03X_W 的单通道图像，默认填充白色 (255)
    cv::Mat binImage(LCDH, LCDW, CV_8UC1, cv::Scalar(255)); 
    // 从最后一行（索引 69）开始，向上绘制 Search_Stop_Line 行，注意下界不能低于 0
    int startRow = 69;  
    
    // int endRow = std::max((uint8_t)0, end_hang);  
    for (int i = startRow; i >= 0; --i) {
        if (L_black[i] >= 0 && L_black[i] < LCDW) 
            binImage.at<uchar>(69-i, 185-L_black[i]) = 0;  // 左边界 黑色
        if (R_black[i] >= 0 && R_black[i] < LCDW) 
            binImage.at<uchar>(69-i, 185-R_black[i]) = 0; // 右边界 黑色
        if (LCenter[i] >= 0 && LCenter[i] < LCDW) 
            binImage.at<uchar>(69-i, 185-LCenter[i]) = 128;  // 中线 灰色 (128)
    }

    std::string folderPath = "./fitting";
    // 生成文件路径
    std::string filePath = folderPath + "/" + std::to_string(index) + ".bmp";

    cv::imwrite(filePath, binImage);
}



void Data_save_to_file(int index)
{
    std::string filePath = "./data/" + std::to_string(index) + ".txt";
    std::ofstream file(filePath);
     if (file.is_open()) {
        file << "[反馈数据]\n"; // 写入文件头
        file << "0=" << current_speed_l << "\n";
        file << "1=" << current_speed_r << "\n";
        file << "2=" << current_speed << "\n";
        file << "3=" << speed_setPoint_l << "\n";
        file << "4=" << speed_setPoint_r << "\n";
        file << "5=" << Left_encoder_data_kal << "\n";
        file << "6=" << Right_encoder_data_kal << "\n";
        file << "7=" << image_error << "\n";
        file << "8=" << duoji_p << "\n";
        file << "9=" << duoji_d << "\n";
        file << "10=" << pid_output << "\n";
        file << "11=" << ramp_flag << "\n";//
        file << "12=" << (uint)road_state << "\n";
        file << "13=" << (uint)huandao_flag_right <<  "\n";
        file << "14=" << (uint)huandao_flag_left <<  "\n";
        file << "15=" << (uint)dajiao << "\n";
        file << "16=" << pid_output_l << "\n";
        file << "17=" << pid_output_r << "\n";
        file << "18=" << speed_p_l_output << "\n";
        file << "19=" << speed_i_l_output << "\n";
        file << "20=" << speed_d_l_output << "\n";
        file << "21=" << speed_p_r_output << "\n";
        file << "22=" << speed_i_r_output<< "\n";
        file << "23=" << speed_d_r_output << "\n";
        file << "24=" << (uint)Road_flag << "\n";
        file << "25=" << imu_gyro_z << "\n";
        file << "26=" << ackerman_left << "\n";
        file << "27=" << ackerman_right << "\n";
        file << "28=" << ackerman_k << "\n";
        file << "29=" << otsuThreshold << "\n";
        file << "30=" << (uint)banma_up_hang << "\n";
        file << "31=" << (uint)xie_shizi_flag_right << "\n";        
        file << "32=" << (uint)xie_shizi_flag_left << "\n";
        file << "33=" << (uint)zr_shizi_flag << "\n";   
        file << "34=" << (uint)during_time << "\n";  
        file << "35=" << fps << "\n"; 
        file << "36=" << (uint)hang_max << "\n";   
        file << "37=" << speed_l_integral << "\n";   
        file << "38=" << speed_r_integral << "\n";   
        file << "39=" << derivative << "\n";  
        file << "40=" << Kp << "\n";   
        file << "41=" << Kd << "\n";   
        file << "42=" << output << "\n";  
        file << "43=" << accumulated_distance << "\n";  

        // file << "36=" << (uint)during_time << "\n"; 
        // file << "37=" << battery_vol << "\n"; 
        
        
        // file << "35=" << hang_len_max_tf << "\n";
        // file << "36=" << (uint)xielv_use_tf << "\n";
        
         
        

        // file << "30=" << delta_output_l << "\n";
        // file << "31=" << delta_output_r << "\n";
        //delta_output
        
        
        
        file.close();
        //std::cout << "数据已写入 " << filePath << std::endl;
    } else {
        std::cerr << "无法打开文件 " << filePath << std::endl;
    }
}

void saveTask()
{
    Binary_image_save(img_index);
    Fitting_image_save(img_index);
    Data_save_to_file(img_index);    
    img_index++;  // 递增索引
}



// 读取 ./origin 文件夹下的所有 BMP 文件
std::vector<std::string> getBmpFiles(const std::string& folderPath) {
    std::vector<std::string> files;
    DIR *dir;
    struct dirent *ent;
    if ((dir = opendir(folderPath.c_str())) != NULL) {
        while ((ent = readdir(dir)) != NULL) {
            std::string filename = ent->d_name;
            if (filename.find(".bmp") != std::string::npos) {
                files.push_back(folderPath + "/" + filename);
            }
        }
        closedir(dir);
    }
    return files;
}

// 显示 origin 图像（二值化）
void showOriginImage(const std::string& filePath) {
    cv::Mat img = cv::imread(filePath, cv::IMREAD_GRAYSCALE);
    if (img.empty()) {
        printf("Failed to load image: %s\n", filePath.c_str());
        return;
    }

    // 转换为 uint8_t 数组（假设 IPS 需要单通道 8-bit 数据）
    uint8_t pixelData[LCDH][LCDW];
    for (int i = 0; i < LCDH; i++) {
        for (int j = 0; j < LCDW; j++) {
            pixelData[i][j] = img.at<uchar>(i, j);
        }
    }

    // 显示到 IPS 屏幕（Y 偏移 60）
    ips200_show_gray_image(0, 20, (uint8_t *)pixelData, LCDW, LCDH);
}

// 显示 fitting 图像（左边界=黑，中线=灰，背景=白）
void showFittingImage(const std::string& filePath) {
    cv::Mat img = cv::imread(filePath, cv::IMREAD_GRAYSCALE);
    if (img.empty()) {
        printf("Failed to load image: %s\n", filePath.c_str());
        return;
    }

    // 遍历像素，绘制边界点
    for (int i = 0; i < LCDH; i++) {
        for (int j = 0; j < LCDW; j++) {
            uchar pixel = img.at<uchar>(i, j);
            if (pixel == 0) {  // 左边界或右边界（黑色）
                ips200_draw_point(j, i + 20, RGB565_MAGENTA);  // 用红色表示
            } else if (pixel == 128) {  // 中线（灰色）
                ips200_draw_point(j, i + 20, RGB565_BROWN);  // 用绿色表示
            }
            // 背景（白色）不绘制
        }
    }

    // 用点绘制方式实现水平线（保持原逻辑不变）
    int screen_y = 20 + (69 - dataValues[15]); // 转换为屏幕坐标（0=最下面，69=最上面）
    for (int x = 0; x < LCDW; x++) {
        ips200_draw_point(x, screen_y, RGB565_YELLOW); // 逐点绘制黄色水平线
    }

//     screen_y = 20 + (69 - dataValues[36]); // 转换为屏幕坐标（0=最下面，69=最上面）
//     for (int x = 0; x < LCDW; x++) {
//         ips200_draw_point(x, screen_y, RGB565_BROWN); // 逐点绘制黄色水平线
//     }
}


// // 显示 fitting 图像（左边界=黑，中线=灰，背景=白）
// void showFittingImage(const std::string& filePath) {
//     std::cout << "[DEBUG] 开始加载图像: " << filePath << std::endl;  // 打印文件路径

//     cv::Mat img = cv::imread(filePath, cv::IMREAD_GRAYSCALE);
//     if (img.empty()) {
//         std::cerr << "[ERROR] 图像加载失败: " << filePath << " (可能文件不存在或格式错误)" << std::endl;
//         return;
//     }
//     std::cout << "[DEBUG] 图像加载成功, 尺寸: " << img.cols << "x" << img.rows 
//               << " (预期: " << LCDW << "x" << LCDH << ")" << std::endl;

//     // 检查图像尺寸是否匹配预期
//     if (img.cols != LCDW || img.rows != LCDH) {
//         std::cerr << "[ERROR] 图像尺寸不匹配! 实际=" << img.cols << "x" << img.rows 
//                   << ", 预期=" << LCDW << "x" << LCDH << std::endl;
//         return;
//     }

//     // 遍历像素，绘制边界点
//     try {
//         std::cout << "[DEBUG] 开始绘制像素..." << std::endl;
//         for (int i = 0; i < img.rows; i++) {
//             for (int j = 0; j < img.cols; j++) {
//                 uchar pixel = img.at<uchar>(i, j);  // 注意OpenCV是(row,col)顺序！
//                 if (pixel == 0) {  // 左边界或右边界（黑色）
//                     ips200_draw_point(j, i + 20, RGB565_MAGENTA);
//                 } else if (pixel == 128) {  // 中线（灰色）
//                     ips200_draw_point(j, i + 20, RGB565_BROWN);
//                 }
//             }
//         }
//         std::cout << "[DEBUG] 像素绘制完成" << std::endl;
//     } catch (const cv::Exception& e) {
//         std::cerr << "[ERROR] OpenCV异常: " << e.what() << std::endl;
//         return;
//     } catch (...) {
//         std::cerr << "[ERROR] 未知异常（可能内存访问错误）" << std::endl;
//         return;
//     }

//     // 绘制水平线（添加边界检查）
//     try {
//         std::cout << "[DEBUG] 准备绘制水平线，dataValues[15]=" << dataValues[15] << std::endl;
//         int screen_y = 20 + (69 - static_cast<int>(dataValues[15]));
//         if (screen_y < 0 || screen_y >= LCDH) {
//             std::cerr << "[WARN] 无效的Y坐标: " << screen_y << " (已跳过)" << std::endl;
//         } else {
//             for (int x = 0; x < LCDW; x++) {
//                 ips200_draw_point(x, screen_y, RGB565_YELLOW);
//             }
//         }

//         std::cout << "[DEBUG] 准备绘制第二条线，dataValues[34]=" << dataValues[34] << std::endl;
//         screen_y = 20 + (69 - static_cast<int>(dataValues[36]));
//         if (screen_y < 0 || screen_y >= LCDH) {
//             std::cerr << "[WARN] 无效的Y坐标: " << screen_y << " (已跳过)" << std::endl;
//         } else {
//             for (int x = 0; x < LCDW; x++) {
//                 ips200_draw_point(x, screen_y, RGB565_BROWN);
//             }
//         }
//     } catch (...) {
//         std::cerr << "[ERROR] 绘制水平线时发生异常" << std::endl;
//     }

//     std::cout << "[DEBUG] showFittingImage 完成" << std::endl;
// }

// 检查文件是否为BMP
bool isBmpFile(const std::string &filename) {
    return filename.size() >= 4 && 
           filename.substr(filename.size() - 4) == ".bmp";
}

// 按数字顺序获取BMP文件
std::vector<std::string> getSortedBmpFiles(const std::string &dirPath) {
    std::vector<std::string> files;
    DIR *dir = opendir(dirPath.c_str());
    
    if (!dir) {
        printf("Cannot open directory: %s\n", dirPath.c_str());
        return files;
    }

    struct dirent *entry;
    while ((entry = readdir(dir)) != nullptr) {
        std::string filename(entry->d_name);
        if (isBmpFile(filename)) {
            files.push_back(dirPath + "/" + filename);
        }
    }
    closedir(dir);

    // 按数字排序（假设文件名是 1.bmp, 2.bmp,...）
    std::sort(files.begin(), files.end(), [](const std::string &a, const std::string &b) {
        int numA = std::stoi(a.substr(a.find_last_of('/') + 1, a.find_last_of('.') - a.find_last_of('/') - 1));
        int numB = std::stoi(b.substr(b.find_last_of('/') + 1, b.find_last_of('.') - b.find_last_of('/') - 1));
        return numA < numB;
    });

    return files;
}


// 专用读取排序后的txt文件
std::vector<std::string> getSortedTxtFiles(const std::string& dirPath) {
    std::vector<std::string> files;
    DIR *dir = opendir(dirPath.c_str());
    
    if (!dir) {
        printf("Cannot open directory: %s\n", dirPath.c_str());
        return files;
    }

    struct dirent *entry;
    while ((entry = readdir(dir)) != nullptr) {
        std::string filename(entry->d_name);
        // 修改为检查.txt后缀
        if (filename.size() >= 4 && 
            filename.substr(filename.size() - 4) == ".txt") {
            files.push_back(dirPath + "/" + filename);
        }
    }
    closedir(dir);

    // 按数字排序（提取文件名中的数字）
    std::sort(files.begin(), files.end(), [](const std::string &a, const std::string &b) {
        auto getNum = [](const std::string &path) {
            size_t slash_pos = path.find_last_of('/');
            size_t dot_pos = path.find_last_of('.');
            return std::stoi(path.substr(slash_pos + 1, dot_pos - slash_pos - 1));
        };
        return getNum(a) < getNum(b);
    });

    return files;
}


bool parseDataFile(const std::string& filePath) {
    std::ifstream file(filePath);
    if (!file.is_open()) {
        printf("无法打开文件: %s (错误: %s)\n", filePath.c_str(), strerror(errno));
        return false;
    }

    std::string line;
    while (std::getline(file, line)) {
        // 跳过空行和注释行
        if (line.empty() || line[0] == '[') continue;
        
        // 解析"数字=值"格式
        size_t eq_pos = line.find('=');
        if (eq_pos == std::string::npos || eq_pos >= line.length() - 1) {
            printf("无效的行格式: %s\n", line.c_str());
            continue;
        }

        try {
            int index = std::stoi(line.substr(0, eq_pos));
            float value = std::stof(line.substr(eq_pos + 1));
            if (index >= 0 && index < Data_num) {
                dataValues[index] = value;
            } else {
                //printf("索引越界: %d (允许范围: 0-%d)\n", index, Data_num - 1);
            }
        } catch (const std::exception& e) {
            printf("解析行失败: %s (错误: %s)\n", line.c_str(), e.what());
            continue;
        }
    }
    file.close();
    return true;
}


void refreshDisplay(size_t index,  // 改为无符号类型
    const std::vector<std::string>& originFiles,
    const std::vector<std::string>& fittingFiles,
    const std::vector<std::string>& dataFiles) {

    //printf("index1: %zu\n", index);  // 调试输出当前索引
    
    // 显示图片（无需检查负数，size_t不会为负）
    showOriginImage(originFiles[index]);

    //printf("index2: %zu\n", index);  // 调试输出当前索引

    showFittingImage(fittingFiles[index]);

    //printf("index3: %zu\n", index);  // 调试输出当前索引

    if (index < dataFiles.size()) {
        std::string dataPath = "./data/" + std::to_string(index + 1) + ".txt";
        if (parseDataFile(dataPath)) {

            //printf("index4: %zu\n", index);  // 调试输出当前索引

            ips200_show_string(0, 90, "huan_r:"); // 显示数据标题
            ips200_show_float(60, 90, dataValues[13], 3 ,1);  
            ips200_show_string(0, 110, "huan_l:");
            ips200_show_float(60, 110, dataValues[14], 1 ,1); 
            ips200_show_string(0, 130, "road_s:");
            ips200_show_float(60, 130, dataValues[12], 3 ,1); 
            //十字
            ips200_show_string(0, 150, "xsr:");
            ips200_show_float(60, 150, dataValues[31], 3 ,1); 
            ips200_show_string(0, 170, "xsl:");
            ips200_show_float(60, 170, dataValues[32], 3 ,1); 
            ips200_show_string(0, 190, "zr:");
            ips200_show_float(60, 190, dataValues[33], 3 ,1); 
            //斑马线
            ips200_show_string(0, 210, "banm:");
            ips200_show_float(60, 210, dataValues[30], 3 ,1); 
            //小s
            ips200_show_string(0, 230, "adc:");
            ips200_show_float(60, 230, dataValues[35], 3 ,1); 

            ips200_show_string(0, 250, "--------------"); // 分隔线

            //扫线
            ips200_show_string(0+120, 0, "erro:");
            ips200_show_float(60+120, 0, dataValues[7], 3 ,1); 
            ips200_show_string(0+120, 90, "dajiao:");
            ips200_show_float(60+120, 90, dataValues[15], 3 ,1); 
            ips200_show_string(0+120, 110, "dj_p:");
            ips200_show_float(60+120, 110, dataValues[8], 3 ,1); 
            ips200_show_string(0+120, 130, "dj_d:");
            ips200_show_float(60+120, 130, dataValues[9], 3 ,1); 
            ips200_show_string(0+120, 150, "dj:");
            ips200_show_float(60+120, 150, dataValues[10], 3 ,1); 
            ips200_show_string(0+120, 170, "--------------"); // 分隔线
            ips200_show_string(0+120, 190, "avg_v:");
            ips200_show_float(60+120, 190, dataValues[2], 3 ,1); 
            ips200_show_string(0+120, 210, "l_v:");
            ips200_show_float(60+120, 210, dataValues[5], 3 ,1); 
            ips200_show_string(0+120, 230, "r_v:");
            ips200_show_float(60+120, 230, dataValues[6], 3 ,1); 
            ips200_show_string(0+120, 250, "set_v:");
            ips200_show_float(60+120, 250, dataValues[3], 3 ,1); 
            ips200_show_string(0+120, 270, "l_pid:");
            ips200_show_float(60+120, 270, dataValues[16], 4 ,1); 
            ips200_show_string(0+120, 290, "r_pid:");
            ips200_show_float(60+120, 290, dataValues[17], 4 ,1);

        }
    }

    // 屏幕坐标计算（显式转换回int）
    int screen_y = 60 + (69 - static_cast<int>(dataValues[15]));
    ips200_draw_line(0, screen_y, LCDW - 1, screen_y, RGB565_YELLOW);

    // 显示当前序号
    ips200_show_string(0, 0, "index:");
    ips200_show_uint(50, 0, index + 1, 3);
}




void bofang_image(void) {
    // 分别用专用函数获取各目录文件
    std::vector<std::string> originFiles = getSortedBmpFiles("./origin");
    std::vector<std::string> fittingFiles = getSortedBmpFiles("./fitting");
    std::vector<std::string> dataFiles = getSortedTxtFiles("./data");  // 使用新函数
    
    if (originFiles.empty() || fittingFiles.empty()) {
        printf("Error: Check origin/ and fitting/ directories\n");
        return;
    }

    int i = 0;
    bool isPlaying = false;

    while (!should_exit) {
        char key = detect_key();
        
        // 按键处理
        if (key == KEY_BACK_M) {
            break;
        } else if (key == KEY_ENTER_M) {
            isPlaying = !isPlaying;
        } else if (key == KEY_UP_M) {
            i = (i + 5) % std::min({originFiles.size(), fittingFiles.size(), dataFiles.size()});
            refreshDisplay(i, originFiles, fittingFiles, dataFiles);
        } else if (key == KEY_DOWN_M) {
            i = (i - 5 + std::min({originFiles.size(), fittingFiles.size(), dataFiles.size()})) % 
                std::min({originFiles.size(), fittingFiles.size(), dataFiles.size()});
            refreshDisplay(i, originFiles, fittingFiles, dataFiles);
        } else if (key == KEY_LEFT_M) {
            i = (i - 1) % std::min({originFiles.size(), fittingFiles.size(), dataFiles.size()});
            refreshDisplay(i, originFiles, fittingFiles, dataFiles);
        } else if (key == KEY_RIGHT_M) {
            i = (i + 1 + std::min({originFiles.size(), fittingFiles.size(), dataFiles.size()})) % 
                std::min({originFiles.size(), fittingFiles.size(), dataFiles.size()});
            refreshDisplay(i, originFiles, fittingFiles, dataFiles);
        }

        // 自动播放
        if (isPlaying) {
            refreshDisplay(i, originFiles, fittingFiles, dataFiles);
            system_delay_ms(50);
            i = (i + 1) % std::min({originFiles.size(), fittingFiles.size(), dataFiles.size()});
        }
    }
}